package com.nfha.algorithm.geo.navigation;

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.Deque;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;

import com.nfha.algorithm.Exerciser;

/**
 * @author Nfha
 *
 */
public class AstarNavigationExerciser extends Exerciser {

	
	public int xScale=400;
	public int yScale=400;
	public double nodeSize=1;
	public double startX=0;
	public double startY=0;
	
	public AstarNavigationExerciser(){
		super();
	}
	
	
	
	
	public AstarNavigationExerciser(int nThreads) {
		super(nThreads);
		// TODO Auto-generated constructor stub
	}




	
	/**  
	 * @param startPoint   起点地理坐标
	 * @param endPoint   终点点地理坐标
	 * @param polygonGeoPoints   多边形地理位置数组 [[lon,lat],[lon,lat],[lon,lat]]
	 * @return   路线节点集合 ，找不到路线返回null
	 * @throws Exception
	 */
	public LinkedList<GeoMapNode> exec(GeoPoint startPoint,GeoPoint endPoint,double[][] polygonGeoPoints) throws Exception{
		
		if(!isPtInPolygon(startPoint, polygonGeoPoints)) {
			throw new Exception("起点["+startPoint.lon+","+startPoint.lat+"]不在测算范围内");
		}else if(!isPtInPolygon(endPoint, polygonGeoPoints)) {
			throw new Exception("终点["+startPoint.lon+","+startPoint.lat+"]不在测算范围内");
		}
		
		LinkedList<GeoMapNode> ret=new LinkedList<>();
		GeoMap geoMap=new GeoMap(xScale, yScale, nodeSize, startX, startY,polygonGeoPoints);
		GeoMapNode startNode=geoMap.getMapNodeByGeo(startPoint.lon, startPoint.lat)
				,endNode=geoMap.getMapNodeByGeo(endPoint.lon, endPoint.lat)
				;
		ret=pathPlanning(startNode, endNode,geoMap);
		logger.info(startNode,"-->",endNode);
		return ret;
	}
	
	  
       /**
     * @param startPoint  起点地理坐标
     * @param endPoint   终点点地理坐标
     * @param geoMap    自行构造或读取持久化的地图对象
     * @return  路线节点集合 ，找不到路线返回null
     * @throws Exception
     */
    public LinkedList<GeoMapNode> exec(GeoPoint startPoint,GeoPoint endPoint,GeoMap geoMap) throws Exception{
		
		if(!isPtInPolygon(startPoint, geoMap.polygonGeoPoints)) {
			throw new Exception("起点["+startPoint.lon+","+startPoint.lat+"]不在测算范围内");
		}else if(!isPtInPolygon(endPoint, geoMap.polygonGeoPoints)) {
			throw new Exception("终点["+startPoint.lon+","+startPoint.lat+"]不在测算范围内");
		}
		
		LinkedList<GeoMapNode> ret=new LinkedList<>();
		
		GeoMapNode startNode=geoMap.getMapNodeByGeo(startPoint.lon, startPoint.lat)
				,endNode=geoMap.getMapNodeByGeo(endPoint.lon, endPoint.lat)
				;
		ret=pathPlanning(startNode, endNode,geoMap);
		logger.info(startNode,"-->",endNode);
		return ret;
	}

	
	/**
	 * A*算法
	 * @param from 开始节点
	 * @param to 终止节点
	 */
	protected LinkedList<GeoMapNode> pathPlanning(GeoMapNode from, GeoMapNode to,GeoMap map){
		 HashSet<GeoMapNode> closeList=new HashSet<>();
		 HashSet<GeoMapNode> openList=new HashSet<>();
		to = map.node2GeoMapNode(to);
		from = map.node2GeoMapNode(from);
		if(!from.isReachable() || !to.isReachable())
			return null;
		if(map.node2GeoMapNode(from) == null || map.node2GeoMapNode(to) == null)
			return null;
		//将起点压入openList
		openList.add(from);
		//如果openList不空，则循环
		while(!openList.isEmpty()){
//			System.out.println(openList);
			//取openList中最佳节点
			GeoMapNode currentNode = getBestNode(openList);
			//获取该节点邻居
			HashSet<GeoMapNode> neighbors = map.getNeighbors(currentNode);
			//若邻居中包含终点，则终止
			if(neighbors.contains(to)){
				to.setFatherNode(currentNode);
				break;
			}
			for(GeoMapNode neighbor:neighbors){
				//如果邻居在close表中，则
				if(closeList.contains(neighbor))
					continue;
				neighbor.setH(to);
				//若该邻居节点存在于open表中
				if(openList.contains(neighbor)){
					GeoMapNode n = getNodeFromList(neighbor, openList);
					double H = neighbor.calH(currentNode);
					//若该邻居节点的G值更小，则替换
					if(H < n.getH()){
						if(isPtInPolygon(currentNode,map)) {
						  neighbor.setFatherNode(currentNode);
						  openList.add(neighbor);
						}
					}
				}
				else if(isPtInPolygon(currentNode,map)){
					neighbor.setFatherNode(currentNode);
					openList.add(neighbor);
				}
			}
			closeList.add(currentNode);
			openList.remove(currentNode);
		}
		return getResult(from, to);
	}
	
	

	/**
	 * 从计算好的Map中输出结果
	 * @param from
	 * @param to
	 * @return
	 */
	protected LinkedList<GeoMapNode> getResult(GeoMapNode from,GeoMapNode to){
		LinkedList<GeoMapNode> result = new LinkedList<GeoMapNode>();
		GeoMapNode node = to;
		while(!node.equals(from)){
			if(node.getFatherNode() == null)
				return null;
			result.push(node);
			node = node.getFatherNode();
		}
		return result;
	}
	
	/**
	 * 从集合中选取G值最小的节点
	 * @param set
	 * @return
	 */
	protected GeoMapNode getBestNode(HashSet<GeoMapNode> set){
		GeoMapNode bestNode = null;
		double bestG = Double.MAX_VALUE;
		for(GeoMapNode node: set){
			if(node.getG() < bestG){
				bestG = node.getG();
				bestNode = node;
			}
		}
		return bestNode;
	}
	
	protected GeoMapNode getNodeFromList(GeoMapNode node, HashSet<GeoMapNode> openList){
		for(GeoMapNode n:openList){
			if(n.equals(node))
				return n;
		}
		return null;
	}
	
	private boolean isPtInPolygon(GeoPoint geoPoint, double[][] polygonGeoPoints) {
		// TODO Auto-generated method stub
		boolean ret=false;
		Point2D.Double point=new Point2D.Double(geoPoint.lon, geoPoint.lat);
		List<java.awt.geom.Point2D.Double> polygon=new ArrayList<>();
		for(double[] polygonGeoPoint : polygonGeoPoints) {
			polygon.add(new Point2D.Double(polygonGeoPoint[0],polygonGeoPoint[1]));
		}
		ret=isPtInPolygon(point, polygon);
		return ret;
	}
	
	private boolean isPtInPolygon(GeoMapNode currentNode, GeoMap map) {
		// TODO Auto-generated method stub
		boolean ret=false;
		Point2D.Double point=new Point2D.Double(currentNode.lon, currentNode.lat);
		List<java.awt.geom.Point2D.Double> polygon=new ArrayList<>();
		for(double[] polygonGeoPoint : map.polygonGeoPoints) {
			polygon.add(new Point2D.Double(polygonGeoPoint[0],polygonGeoPoint[1]));
		}
		ret=isPtInPolygon(point, polygon);
		return ret;
	}
	
	
	
	public static boolean isPtInPolygon (Point2D.Double point , List<Point2D.Double> polygon) {
        
 
        int iSum,iIndex;
        double dLon1 , dLon2 , dLat1 , dLat2 , dLon;
        int size = polygon.size();
        iSum = 0;
        for (iIndex = 0; iIndex<size; iIndex++) {
            if (iIndex == size - 1) {
                dLon1 = polygon.get(iIndex).getX();
                dLat1 = polygon.get(iIndex).getY();
                dLon2 = polygon.get(0).getX();
                dLat2 = polygon.get(0).getY();
            } else {
                dLon1 = polygon.get(iIndex).getX();
                dLat1 = polygon.get(iIndex).getY();
                dLon2 = polygon.get(iIndex + 1).getX();
                dLat2 = polygon.get(iIndex + 1).getY();
            }
            // 以下语句判断A点是否在边的两端点的水平平行线之间，在则可能有交点，开始判断交点是否在左射线上
            if (((point.y >= dLat1) && (point.y < dLat2))
                    || ((point.y >= dLat2) && (point.y < dLat1))) {
                if (Math.abs(dLat1 - dLat2) > 0) {
                    //得到 A点向左射线与边的交点的x坐标：
                    dLon = dLon1 - ((dLon1 - dLon2) * (dLat1 - point.y) ) / (dLat1 - dLat2);
                    // 如果交点在A点左侧（说明是做射线与 边的交点），则射线与边的全部交点数加一：
                    if (dLon < point.x) {
                        iSum++;
                    }
                }
            }
        }
        return (iSum % 2) != 0;
    }

	
	
	
	
}